#include "Maths.h"

using namespace maths;

CVector4f::CVector4f()
{
	__asm
	{
		xorps xmm0, xmm0
		mov eax, dword ptr this
		movaps xmmword ptr [eax], xmm0
	}
}

CVector4f::CVector4f(float a_X, float a_Y, float a_Z, float a_W)
{
	m_Data[0] = a_X;
	m_Data[1] = a_Y;
	m_Data[2] = a_Z;
	m_Data[3] = a_W;
}

CVector4f::CVector4f(const CVector4f& a_V)
{
	__asm
	{
		mov eax, dword ptr a_V
		movaps xmm0, xmmword ptr [eax]
		mov eax, dword ptr this
		movaps xmmword ptr [eax], xmm0
	}
}

const float CVector4f :: operator [] (int a_Index) const
{
	return m_Data[a_Index];
}

float& CVector4f :: operator [](int a_Index) 
{
	return m_Data[a_Index];
}

CVector4f& CVector4f :: operator  = (const CVector4f& a_V)
{
	__asm
	{
		mov eax, dword ptr a_V
		movaps xmm0, xmmword ptr [eax]
		mov eax, dword ptr this
		movaps xmmword ptr [eax], xmm0
	}
	return *this;
}

CVector4f& CVector4f :: operator += (const CVector4f& a_V)
{
	__asm
	{
		mov eax, dword ptr a_V
		movaps xmm0, xmmword ptr [eax]
		mov eax, dword ptr this
		movaps xmm1, xmmword ptr [eax]
		addps xmm0, xmm1
		movaps xmmword ptr [eax], xmm0
	}
	return *this;
}

CVector4f& CVector4f :: operator -= (const CVector4f& a_V)
{
	__asm
	{
		mov eax, dword ptr a_V
		movaps xmm0, xmmword ptr [eax]
		mov eax, dword ptr this
		movaps xmm1, xmmword ptr [eax]
		subps xmm0, xmm1
		movaps xmmword ptr [eax], xmm0
	}
	return *this;
}

CVector4f& CVector4f :: operator ^= (const CVector4f& a_V)
{
	__asm
	{
		mov eax, dword ptr a_V
		movaps xmm2, xmmword ptr [eax] // w z y x
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax] // d c b a
		pshufd xmm1, xmm0, 0xD2        // d b a c
		pshufd xmm0, xmm0, 0xC9        // d a c b
		pshufd xmm3, xmm2, 0xC9        // w x z y
		pshufd xmm2, xmm2, 0xD2        // w y x z
		mulps  xmm0, xmm2              // d*w a*y c*x b*z
		mulps  xmm1, xmm3              // d*w b*x a*z c*y
		subps  xmm0, xmm1              // 0 a*y-b*x c*x-a*z b*z-c*y
		movaps xmmword ptr [eax], xmm0
	}
	return *this;
}

CVector4f& CVector4f :: operator *= (const float a_K)
{
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax]
		movss  xmm1, a_K
		pshufd xmm1, xmm1, 00h
		mulps  xmm0, xmm1
		movaps xmmword ptr [eax], xmm0
	}
	return *this;
}

CVector4f& CVector4f :: operator /= (const float a_K)
{
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax]
		movss   xmm1, a_K
		pshufd xmm1, xmm1, 00h
		divps  xmm0, xmm1
		movaps xmmword ptr [eax], xmm0
	}
	return *this;
}

bool CVector4f :: operator != (const CVector4f& a_V) const
{
	return ((fabs(m_Data[0] - a_V.m_Data[0]) > _FLOAT_EPS_) ||
		(fabs(m_Data[1] - a_V.m_Data[1]) > _FLOAT_EPS_) ||
		(fabs(m_Data[2] - a_V.m_Data[2]) > _FLOAT_EPS_) ||
		(fabs(m_Data[3] - a_V.m_Data[3]) > _FLOAT_EPS_));
}


bool CVector4f :: operator == (const CVector4f& a_V) const
{
	return ((fabs(m_Data[0] - a_V.m_Data[0]) < _FLOAT_EPS_) &&
		(fabs(m_Data[1] - a_V.m_Data[1]) < _FLOAT_EPS_) &&
		(fabs(m_Data[2] - a_V.m_Data[2]) < _FLOAT_EPS_) &&
		(fabs(m_Data[3] - a_V.m_Data[3]) < _FLOAT_EPS_));
}

CVector4f CVector4f :: operator + (const CVector4f& a_V) const
{
	CVector4f l_Result;
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax]
		mov eax, dword ptr a_V
		movaps xmm1, xmmword ptr [eax]
		addps  xmm0, xmm1
		movaps xmmword ptr [l_Result], xmm0
	}
	return l_Result;
}

CVector4f CVector4f :: operator - (const CVector4f& a_V) const
{
	CVector4f l_Result;
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax]
		mov eax, dword ptr a_V
		movaps xmm1, xmmword ptr [eax]
		subps  xmm0, xmm1
		movaps xmmword ptr [l_Result], xmm0
	}
	return l_Result;
}

float CVector4f :: operator * (const CVector4f& a_V) const
{
	float l_Result = 0.0;
	__asm
    {
        mov eax, dword ptr this
        movaps xmm0, xmmword ptr [eax] //xmm0 = a b c d
        mov eax, dword ptr a_V
        movaps xmm2, xmmword ptr [eax] //xmm2 = x y z w
        
        mulps       xmm0, xmm2         //xmm0 = ax by cz dw
        movhlps     xmm1, xmm0         //xmm1 =  ?  ? ax by
        unpcklps    xmm0, xmm0         //xmm0 = cz cz dw dw
        movhlps     xmm3, xmm0         //xmm3 =  ?  ? cz cz
        addss       xmm0, xmm1         //xmm0 = ? ? ? by + dw
        addss       xmm0, xmm3         //xmm0 = ? ? ? by + cz + dw
        movss l_Result, xmm0
	}
	return l_Result;
}

CVector4f CVector4f :: operator ^ (const CVector4f& a_V) const
{
	CVector4f l_Result;
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax] // d c b a
		mov eax, dword ptr a_V
		movaps xmm2, xmmword ptr [eax] // w z y x
		pshufd xmm1, xmm0, 0xD2        // d b a c
		pshufd xmm0, xmm0, 0xC9        // d a c b
		pshufd xmm3, xmm2, 0xC9        // w x z y
		pshufd xmm2, xmm2, 0xD2        // w y x z
		mulps  xmm0, xmm2              // d*w a*y c*x b*z
		mulps  xmm1, xmm3              // d*w b*x a*z c*y
		subps  xmm0, xmm1              // 0 a*y-b*x c*x-a*z b*z-c*y
		movaps xmmword ptr [l_Result], xmm0
	}
	return l_Result;
}

CVector4f CVector4f :: operator * (const float a_K) const
{
	CVector4f l_Result;
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax]
		movss   xmm1, a_K
		pshufd xmm1, xmm1, 00h
		mulps  xmm0, xmm1
		movaps xmmword ptr [l_Result], xmm0
	}
	return l_Result;
}

CVector4f CVector4f :: operator / (const float a_K) const
{
	CVector4f l_Result;
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax]
		movss   xmm1, a_K
		pshufd xmm1, xmm1, 00h
		divps  xmm0, xmm1
		movaps xmmword ptr [l_Result], xmm0
	}
	return l_Result;
}

//functions

CVector4f& CVector4f :: Invert()
{
	__asm
	{
		mov eax, dword ptr this
		movaps xmm0, xmmword ptr [eax]
		xorps xmm1, xmm1
		subps xmm1, xmm0
		movaps xmmword ptr [eax], xmm1
	}
	return *this;
}

CVector4f& CVector4f :: Normalize()
{
	__asm
	{
		mov eax, dword ptr this
		movaps  xmm0, xmmword ptr [eax]
		movaps  xmm1, xmm0
		mulps   xmm0, xmm0
		haddps  xmm0, xmm0
		haddps  xmm0, xmm0
		rsqrtps xmm0, xmm0
		mulps   xmm1, xmm0
		movaps  xmmword ptr [eax], xmm1
	}
	return *this;
}

CVector4f& CVector4f :: SetLength (float a_Length)
{
	__asm
	{
		mov eax, dword ptr this
		movaps  xmm0, xmmword ptr [eax]
		movaps  xmm1, xmm0
		mulps   xmm0, xmm0
		haddps  xmm0, xmm0
		haddps  xmm0, xmm0
		rsqrtps xmm0, xmm0
		mulps   xmm1, xmm0

		movss   xmm0, a_Length
		pshufd  xmm0, xmm0, 0h
		mulps   xmm1, xmm0
		movaps  xmmword ptr [eax], xmm1
	}
	return *this;
}

CVector4f& CVector4f :: operator *= (const CMatrix4x4f& a_M)
{
	__asm
	{
		mov eax, dword ptr a_M
		movaps xmm4, xmmword ptr [eax     ]
		movaps xmm5, xmmword ptr [eax + 16]
		movaps xmm6, xmmword ptr [eax + 32]
		movaps xmm7, xmmword ptr [eax + 48]

		mov eax, dword ptr this
		movaps xmm3, xmmword ptr [eax]
		pshufd xmm0, xmm3, 00000000b
		pshufd xmm1, xmm3, 01010101b
		pshufd xmm2, xmm3, 10101010b
		pshufd xmm3, xmm3, 11111111b

		mulps  xmm0, xmm4
		mulps  xmm1, xmm5
		mulps  xmm2, xmm6
		mulps  xmm3, xmm7

		addps  xmm0, xmm1
		addps  xmm0, xmm2
		addps  xmm0, xmm3

		movaps xmmword ptr [eax], xmm0
	}
	return *this;
}

CVector4f& CVector4f :: operator ^= (const CMatrix4x4f& a_M)
{
	__asm
	{
		mov eax, dword ptr a_M
		movaps xmm0, xmmword ptr [eax     ]
		movaps xmm1, xmmword ptr [eax + 16]
		movaps xmm2, xmmword ptr [eax + 32]
		movaps xmm3, xmmword ptr [eax + 48]

		mov eax, dword ptr this
		movaps xmm4, xmmword ptr [eax]
		
		mulps  xmm0, xmm4
		mulps  xmm1, xmm4
		mulps  xmm2, xmm4
		mulps  xmm3, xmm4

		haddps  xmm0, xmm1
		haddps  xmm2, xmm3
		haddps  xmm0, xmm2

		movaps xmmword ptr [eax], xmm0
	}
	return *this;
}

CVector4f  CVector4f :: operator * (const CMatrix4x4f& a_M) const
{
	CVector4f l_Result;
	__asm
	{
		mov eax, dword ptr a_M
		movaps xmm4, xmmword ptr [eax     ]
		movaps xmm5, xmmword ptr [eax + 16]
		movaps xmm6, xmmword ptr [eax + 32]
		movaps xmm7, xmmword ptr [eax + 48]

		mov eax, dword ptr this
		movaps xmm3, xmmword ptr [eax]
		pshufd xmm0, xmm3, 00000000b
		pshufd xmm1, xmm3, 01010101b
		pshufd xmm2, xmm3, 10101010b
		pshufd xmm3, xmm3, 11111111b

		mulps  xmm0, xmm4
		mulps  xmm1, xmm5
		mulps  xmm2, xmm6
		mulps  xmm3, xmm7

		addps  xmm0, xmm1
		addps  xmm0, xmm2
		addps  xmm0, xmm3

		movaps xmmword ptr [l_Result], xmm0
	}
	return l_Result;
}